d#include <rov/motor.h>
#include <rov/util.h>

#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>

#include <iostream>
#include <sstream>


using namespace std;


static char const * const MOTORS_ID = "motors";
static char const * const DEVICE_PATTERN = "/dev/ttyACM";
static int motorFd = -1;


int initMotors() {
    int fd = -1;

//    while (fd < 0) {
        for (int i = 0; i < 10 && fd < 0; i++) {
            stringstream name;

            name << DEVICE_PATTERN << i;

cerr << "checking device: " << name.str() << endl;

            fd = open(name.str().c_str(), O_RDWR | O_NOCTTY);

            if (fd >= 0 && (configSerial(fd, B57600)
                            || !checkMatches(fd, MOTORS_ID))) {

                close(fd);
                fd = -1;
            }
        }

//        sleep(1);
//    }

    if (fd >= 0)
        motorFd = fd;

    return fd;
}


// returns 0 on success
int setMotorSpeed(Motor m, Speed s) {
    if (motorFd < 0 || m < 0)
        return -1;

    char buf[2];
    //stringstream cmd;
    //cmd << (char) m << (char) s;

    buf[0] = m;
    buf[1] = s;

//cerr << "sending cmd(" << cmd.str().length() << "): " << (int) cmd.str().c_str()[0]
//     << " " << (int) cmd.str().c_str()[1] << endl;
cerr << "sending cmd (" << 2 << "): " << (int) buf[0]
     << " " << (int) buf[1] << endl;

    unsigned int n = 0;
//    while (n < cmd.str().length()) {
    while (n < 2) {
//        int amt = write(motorFd, cmd.str().c_str()+n, cmd.str().length()-n);
        int amt = write(motorFd, &buf[n], 2-n);

        if (amt < 0)
            break;

cerr << "sent: " << amt << endl;

        n += amt;
    }

cerr << "sent total: " << n << endl;

//    return n != cmd.str().length();
    return n != 2;
}


int motorOutput() {
    if (motorFd < 0)
        return -1;

    char buf[1000];

    int amt = read(motorFd, buf, 1000);

    if (amt < 0)
        return -1;

    buf[amt] = 0;

    cerr << "motor output: \"" << buf << "\"\n";

    return 0;
}
